
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       gcs_copter.c
  * @author     baiyang
  * @date       2022-11-10
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "gcs_copter.h"
#include <gcs_mavlink/gcs.h>

#include <rtthread.h>

#include "fms.h"
#include <parameter/param.h>
#include <board_config/borad_config.h>
/*-----------------------------------macro------------------------------------*/
#define GCS_PARAM_VAL_INFO(x) \
    PARAM_DEFINE_INT16(SR##x##_RAW_SENS, 0), \
    PARAM_DEFINE_INT16(SR##x##_EXT_STAT, 0), \
    PARAM_DEFINE_INT16(SR##x##_RC_CHAN, 0), \
    PARAM_DEFINE_INT16(SR##x##_RAW_CTRL, 0), \
    PARAM_DEFINE_INT16(SR##x##_POSITION, 0), \
    PARAM_DEFINE_INT16(SR##x##_EXTRA1, 0), \
    PARAM_DEFINE_INT16(SR##x##_EXTRA2, 0), \
    PARAM_DEFINE_INT16(SR##x##_EXTRA3, 0), \
    PARAM_DEFINE_INT16(SR##x##_PARAMS, 0), \
    PARAM_DEFINE_INT16(SR##x##_ADSB, 0)

#define GCS_PARAM_LINK_VARIABLE(gcs, x) \
    param_link_variable(PARAM_ID(GCS, SR##x##_RAW_SENS), &gcs->chan_parameters[x].streamRates[0]); \
    param_link_variable(PARAM_ID(GCS, SR##x##_EXT_STAT), &gcs->chan_parameters[x].streamRates[1]); \
    param_link_variable(PARAM_ID(GCS, SR##x##_RC_CHAN), &gcs->chan_parameters[x].streamRates[2]); \
    param_link_variable(PARAM_ID(GCS, SR##x##_RAW_CTRL), &gcs->chan_parameters[x].streamRates[3]); \
    param_link_variable(PARAM_ID(GCS, SR##x##_POSITION), &gcs->chan_parameters[x].streamRates[4]); \
    param_link_variable(PARAM_ID(GCS, SR##x##_EXTRA1), &gcs->chan_parameters[x].streamRates[5]); \
    param_link_variable(PARAM_ID(GCS, SR##x##_EXTRA2), &gcs->chan_parameters[x].streamRates[6]); \
    param_link_variable(PARAM_ID(GCS, SR##x##_EXTRA3), &gcs->chan_parameters[x].streamRates[7]); \
    param_link_variable(PARAM_ID(GCS, SR##x##_PARAMS), &gcs->chan_parameters[x].streamRates[8]); \
    param_link_variable(PARAM_ID(GCS, SR##x##_ADSB), &gcs->chan_parameters[x].streamRates[9])

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/
static void task_gcs_entry(void* parameter);

static uint8_t gcs_copter_sysid_this_mav(gcs_t gcs_singleton);
static uint8_t gcs_copter_sysid_my_gcs(gcs_t gcs_singleton);
static bool gcs_copter_vehicle_initialised(gcs_t gcs_singleton);
static void gcs_copter_update_vehicle_sensor_status_flags(gcs_t gcs_singleton);
static uint16_t gcs_copter_min_loop_time_remaining_for_message_send_us(gcs_t gcs_singleton);
static gcs_mavlink_t gcs_copter_new_gcs_mavlink_backend(gcs_t gcs_singleton, gcs_mavlink_parameters_t params, rt_device_t uart);
static gcs_mavlink_t gcsmav_copter_probe(gcs_mavlink_parameters_t params, rt_device_t uart);
static void gcs_copter_assign_param();

/// GCS Mavlink Copter
static uint8_t gcsmav_copter_sysid_my_gcs(gcs_mavlink_t gcsmav);
static bool gcsmav_copter_sysid_enforce(gcs_mavlink_t gcsmav);
static uint64_t gcsmav_copter_capabilities(gcs_mavlink_t gcsmav);
static uint32_t gcsmav_copter_telem_delay(gcs_mavlink_t gcsmav);
static MAV_LANDED_STATE gcsmav_copter_landed_state(gcs_mavlink_t gcsmav);
/*----------------------------------variable----------------------------------*/
static param_t var_info[] = {
    // @Param: RAW_SENS
    // @DisplayName: Raw sensor stream rate
    // @Description: MAVLink Stream rate of RAW_IMU, SCALED_IMU2, SCALED_IMU3, SCALED_PRESSURE, SCALED_PRESSURE2, and SCALED_PRESSURE3
    // @Units: Hz
    // @Range: 0 50
    // @Increment: 1
    // @RebootRequired: True
    // @User: Advanced

    // @Param: EXT_STAT
    // @DisplayName: Extended status stream rate
    // @Description: MAVLink Stream rate of SYS_STATUS, POWER_STATUS, MCU_STATUS, MEMINFO, CURRENT_WAYPOINT, GPS_RAW_INT, GPS_RTK (if available), GPS2_RAW_INT (if available), GPS2_RTK (if available), NAV_CONTROLLER_OUTPUT, FENCE_STATUS, and GLOBAL_TARGET_POS_INT
    // @Units: Hz
    // @Range: 0 50
    // @Increment: 1
    // @RebootRequired: True
    // @User: Advanced

    // @Param: RC_CHAN
    // @DisplayName: RC Channel stream rate
    // @Description: MAVLink Stream rate of SERVO_OUTPUT_RAW and RC_CHANNELS
    // @Units: Hz
    // @Range: 0 50
    // @Increment: 1
    // @RebootRequired: True
    // @User: Advanced

    // @Param: RAW_CTRL
    // @DisplayName: Unused
    // @Description: Unused
    // @Units: Hz
    // @Range: 0 50
    // @Increment: 1
    // @RebootRequired: True
    // @User: Advanced

    // @Param: POSITION
    // @DisplayName: Position stream rate
    // @Description: MAVLink Stream rate of GLOBAL_POSITION_INT and LOCAL_POSITION_NED
    // @Units: Hz
    // @Range: 0 50
    // @Increment: 1
    // @RebootRequired: True
    // @User: Advanced

    // @Param: EXTRA1
    // @DisplayName: Extra data type 1 stream rate
    // @Description: MAVLink Stream rate of ATTITUDE, SIMSTATE (SIM only), AHRS2 and PID_TUNING
    // @Range: 0 50
    // @Increment: 1
    // @RebootRequired: True
    // @User: Advanced

    // @Param: EXTRA2
    // @DisplayName: Extra data type 2 stream rate
    // @Description: MAVLink Stream rate of VFR_HUD
    // @Units: Hz
    // @Range: 0 50
    // @Increment: 1
    // @RebootRequired: True
    // @User: Advanced

    // @Param: EXTRA3
    // @DisplayName: Extra data type 3 stream rate
    // @Description: MAVLink Stream rate of AHRS, SYSTEM_TIME, WIND, RANGEFINDER, DISTANCE_SENSOR, TERRAIN_REQUEST, BATTERY_STATUS, GIMBAL_DEVICE_ATTITUDE_STATUS, OPTICAL_FLOW, MAG_CAL_REPORT, MAG_CAL_PROGRESS, EKF_STATUS_REPORT, VIBRATION, RPM, ESC TELEMETRY,GENERATOR_STATUS, and WINCH_STATUS
    // @Units: Hz
    // @Range: 0 50
    // @Increment: 1
    // @RebootRequired: True
    // @User: Advanced

    // @Param: PARAMS
    // @DisplayName: Parameter stream rate
    // @Description: MAVLink Stream rate of PARAM_VALUE
    // @Units: Hz
    // @Range: 0 50
    // @Increment: 1
    // @RebootRequired: True
    // @User: Advanced

    // @Param: ADSB
    // @DisplayName: ADSB stream rate
    // @Description: MAVLink ADSB stream rate
    // @Units: Hz
    // @Range: 0 50
    // @Increment: 1
    // @RebootRequired: True
    // @User: Advanced

    GCS_PARAM_VAL_INFO(0),

#if MAVLINK_COMM_NUM_BUFFERS >= 2
    GCS_PARAM_VAL_INFO(1),
#endif

#if MAVLINK_COMM_NUM_BUFFERS >= 3
    GCS_PARAM_VAL_INFO(2),
#endif

#if MAVLINK_COMM_NUM_BUFFERS >= 4
    GCS_PARAM_VAL_INFO(3),
#endif

#if MAVLINK_COMM_NUM_BUFFERS >= 5
    GCS_PARAM_VAL_INFO(4),
#endif

#if MAVLINK_COMM_NUM_BUFFERS >= 6
    GCS_PARAM_VAL_INFO(5),
#endif

#if MAVLINK_COMM_NUM_BUFFERS >= 7
    GCS_PARAM_VAL_INFO(6),
#endif

    // @Param: SYSID_THISMAV
    // @DisplayName: MAVLink system ID of this vehicle
    // @Description: Allows setting an individual MAVLink system id for this vehicle to distinguish it from others on the same network
    // @Range: 1 255
    // @User: Advanced
    PARAM_DEFINE_INT16(SYSID_THISMAV,MAV_SYSTEM_ID),

    // @Param: SYSID_MYGCS
    // @DisplayName: My ground station number
    // @Description: Allows restricting radio overrides to only come from my ground station
    // @Range: 1 255
    // @User: Advanced
    PARAM_DEFINE_INT16(SYSID_MYGCS, 255),

    // @Param: SYSID_ENFORCE
    // @DisplayName: GCS sysid enforcement
    // @Description: This controls whether packets from other than the expected GCS system ID will be accepted
    // @Values: 0:NotEnforced,1:Enforced
    // @User: Advanced
    PARAM_DEFINE_INT16(SYSID_ENFORCE, 0),

    // @Param: TELEM_DELAY
    // @DisplayName: Telemetry startup delay
    // @Description: The amount of time (in seconds) to delay radio telemetry to prevent an Xbee bricking on power up
    // @User: Advanced
    // @Units: s
    // @Range: 0 30
    // @Increment: 1
    PARAM_DEFINE_INT8(TELEM_DELAY,0),
};
PARAM_GROUP_DEFINE(GCS, var_info);

static const enum mb_message STREAM_RAW_SENSORS_msgs[] = {
    MAV_MSG_RAW_IMU,
    MAV_MSG_SCALED_IMU2,
    MAV_MSG_SCALED_IMU3,
    MAV_MSG_SCALED_PRESSURE,
    MAV_MSG_SCALED_PRESSURE2,
    MAV_MSG_SCALED_PRESSURE3,
};
static const enum mb_message STREAM_EXTENDED_STATUS_msgs[] = {
    MAV_MSG_SYS_STATUS,
    MAV_MSG_POWER_STATUS,
    MAV_MSG_MCU_STATUS,
    MAV_MSG_MEMINFO,
    MAV_MSG_CURRENT_WAYPOINT, // MISSION_CURRENT
    MAV_MSG_GPS_RAW,
    MAV_MSG_GPS_RTK,
    MAV_MSG_GPS2_RAW,
    MAV_MSG_GPS2_RTK,
    MAV_MSG_NAV_CONTROLLER_OUTPUT,
    MAV_MSG_FENCE_STATUS,
    MAV_MSG_POSITION_TARGET_GLOBAL_INT,
};
static const enum mb_message STREAM_POSITION_msgs[] = {
    MAV_MSG_LOCATION,
    MAV_MSG_LOCAL_POSITION
};
static const enum mb_message STREAM_RC_CHANNELS_msgs[] = {
    MAV_MSG_SERVO_OUTPUT_RAW,
    MAV_MSG_RC_CHANNELS,
    MAV_MSG_RC_CHANNELS_RAW, // only sent on a mavlink1 connection
};
static const enum mb_message STREAM_EXTRA1_msgs[] = {
    MAV_MSG_ATTITUDE,
    MAV_MSG_SIMSTATE,
    MAV_MSG_AHRS2,
    MAV_MSG_PID_TUNING // Up to four PID_TUNING messages are sent, depending on GCS_PID_MASK parameter
};
static const enum mb_message STREAM_EXTRA2_msgs[] = {
    MAV_MSG_VFR_HUD
};
static const enum mb_message STREAM_EXTRA3_msgs[] = {
    MAV_MSG_AHRS,
    MAV_MSG_SYSTEM_TIME,
    MAV_MSG_WIND,
    MAV_MSG_RANGEFINDER,
    MAV_MSG_DISTANCE_SENSOR,
    MAV_MSG_BATTERY_STATUS,
    MAV_MSG_GIMBAL_DEVICE_ATTITUDE_STATUS,
    MAV_MSG_OPTICAL_FLOW,
    MAV_MSG_MAG_CAL_REPORT,
    MAV_MSG_MAG_CAL_PROGRESS,
    MAV_MSG_EKF_STATUS_REPORT,
    MAV_MSG_VIBRATION,
    MAV_MSG_ESC_TELEMETRY,
    MAV_MSG_GENERATOR_STATUS,
    MAV_MSG_WINCH_STATUS,
};
static const enum mb_message STREAM_PARAMS_msgs[] = {
    MAV_MSG_NEXT_PARAM
};
static const enum mb_message STREAM_ADSB_msgs[] = {
    MAV_MSG_ADSB_VEHICLE,
    MAV_MSG_AIS_VESSEL,
};

const struct stream_entries all_stream_entries[] = {
    MAV_STREAM_ENTRY(STREAM_RAW_SENSORS),
    MAV_STREAM_ENTRY(STREAM_EXTENDED_STATUS),
    MAV_STREAM_ENTRY(STREAM_POSITION),
    MAV_STREAM_ENTRY(STREAM_RC_CHANNELS),
    MAV_STREAM_ENTRY(STREAM_EXTRA1),
    MAV_STREAM_ENTRY(STREAM_EXTRA2),
    MAV_STREAM_ENTRY(STREAM_EXTRA3),
    MAV_STREAM_ENTRY(STREAM_ADSB),
    MAV_STREAM_ENTRY(STREAM_PARAMS),
    MAV_STREAM_TERMINATOR // must have this at end of stream_entries
};

static struct gcs_ops gcs_copter_ops = {.vehicle_initialised = gcs_copter_vehicle_initialised,
                                        .sysid_this_mav = gcs_copter_sysid_this_mav,
                                        .sysid_my_gcs = gcs_copter_sysid_my_gcs,
                                        .update_vehicle_sensor_status_flags = gcs_copter_update_vehicle_sensor_status_flags,
                                        .new_gcs_mavlink_backend = gcs_copter_new_gcs_mavlink_backend,
                                        .min_loop_time_remaining_for_message_send_us = gcs_copter_min_loop_time_remaining_for_message_send_us};

struct gcs_mavlink_ops gcsmav_copter_ops = {.sysid_my_gcs = gcsmav_copter_sysid_my_gcs,
                                            .sysid_enforce = gcsmav_copter_sysid_enforce,
                                            .capabilities = gcsmav_copter_capabilities,
                                            .telem_delay = gcsmav_copter_telem_delay,
                                            .landed_state = gcsmav_copter_landed_state};

static char thread_gcs_stack[1024*5] RT_SECTION(".ccmram");
static struct rt_thread thread_gcs_handle;

// Telemetry control
//
static Param_int16        sysid_this_mav;
static Param_int16        sysid_my_gcs;

// whether to enforce acceptance of packets only from sysid_my_gcs
static Param_int8         sysid_enforce;

static Param_int8         telem_delay;
/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
rt_err_t task_gcs_init(void)
{
    rt_err_t res;

    res = rt_thread_init(&thread_gcs_handle,
        "gcs",
        task_gcs_entry,
        RT_NULL,
        &thread_gcs_stack[0],
        sizeof(thread_gcs_stack), PRIORITY_COMM, 5);
    RT_ASSERT(res == RT_EOK);
    rt_thread_startup(&thread_gcs_handle);

    brd_set_vehicle_init_stage(INIT_STAGE_COMM);

    return GP_EOK;
}

static void task_gcs_entry(void* parameter)
{
    rt_err_t res;

    while (1) {
        gcs_update_receive();
        gcs_update_send();

        rt_thread_mdelay(2);
    }
}

static uint8_t gcs_copter_sysid_this_mav(gcs_t gcs_singleton)
{
    return sysid_this_mav;
}

static uint8_t gcs_copter_sysid_my_gcs(gcs_t gcs_singleton)
{
    return sysid_my_gcs;
}

static bool gcs_copter_vehicle_initialised(gcs_t gcs_singleton)
{
    return fms.ap.initialised;
}

static void gcs_copter_update_vehicle_sensor_status_flags(gcs_t gcs_singleton)
{
    gcs_singleton->control_sensors_present |=
        MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL |
        MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION |
        MAV_SYS_STATUS_SENSOR_YAW_POSITION;

    gcs_singleton->control_sensors_enabled |=
        MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL |
        MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION |
        MAV_SYS_STATUS_SENSOR_YAW_POSITION;

    gcs_singleton->control_sensors_health |=
        MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL |
        MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION |
        MAV_SYS_STATUS_SENSOR_YAW_POSITION;

    const ap_t *ap = &fms.ap;

    if (ap->rc_receiver_present) {
        gcs_singleton->control_sensors_present |= MAV_SYS_STATUS_SENSOR_RC_RECEIVER;
        gcs_singleton->control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_RC_RECEIVER;
    }
    if (ap->rc_receiver_present && !fms.failsafe.radio) {
        gcs_singleton->control_sensors_health |= MAV_SYS_STATUS_SENSOR_RC_RECEIVER;
    }

    // update flightmode-specific flags:
    gcs_singleton->control_sensors_present |= MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL;
    gcs_singleton->control_sensors_present |= MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL;

    switch (mode_number(fms.flightmode)) {
    case AUTO:
    case AUTO_RTL:
    case AVOID_ADSB:
    case GUIDED:
    case LOITER:
    case RTL:
    case CIRCLE:
    case LAND:
    case POSHOLD:
    case BRAKE:
    case THROW:
    case SMART_RTL:
        gcs_singleton->control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL;
        gcs_singleton->control_sensors_health |= MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL;
        gcs_singleton->control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL;
        gcs_singleton->control_sensors_health |= MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL;
        break;
    case ALT_HOLD:
    case GUIDED_NOGPS:
    case SPORT:
    case AUTOTUNE:
    case FLOWHOLD:
        gcs_singleton->control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL;
        gcs_singleton->control_sensors_health |= MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL;
        break;
    default:
        // stabilize, acro, drift, and flip have no automatic x,y or z control (i.e. all manual)
        break;
    }

#if 0
    // optional sensors, some of which are essentially always
    // available in the firmware:
#if HAL_PROXIMITY_ENABLED
    if (copter.g2.proximity.sensor_present()) {
        control_sensors_present |= MAV_SYS_STATUS_SENSOR_PROXIMITY;
    }
    if (copter.g2.proximity.sensor_enabled()) {
        control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_PROXIMITY;
    }
    if (!copter.g2.proximity.sensor_failed()) {
        control_sensors_health |= MAV_SYS_STATUS_SENSOR_PROXIMITY;
    }
#endif

#if RANGEFINDER_ENABLED == ENABLED
    const RangeFinder *rangefinder = RangeFinder::get_singleton();
    if (rangefinder && rangefinder->has_orientation(ROTATION_PITCH_270)) {
        control_sensors_present |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;
    }
    if (copter.rangefinder_state.enabled) {
        control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;
        if (rangefinder && rangefinder->has_data_orient(ROTATION_PITCH_270)) {
            control_sensors_health |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;
        }
    }
#endif

#if PRECISION_LANDING == ENABLED
    if (copter.precland.enabled()) {
        control_sensors_present |= MAV_SYS_STATUS_SENSOR_VISION_POSITION;
        control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_VISION_POSITION;
    }
    if (copter.precland.enabled() && copter.precland.healthy()) {
        control_sensors_health |= MAV_SYS_STATUS_SENSOR_VISION_POSITION;
    }
#endif

#if AP_TERRAIN_AVAILABLE
    switch (copter.terrain.status()) {
    case AP_Terrain::TerrainStatusDisabled:
        break;
    case AP_Terrain::TerrainStatusUnhealthy:
        // To-Do: restore unhealthy terrain status reporting once terrain is used in copter
        //control_sensors_present |= MAV_SYS_STATUS_TERRAIN;
        //control_sensors_enabled |= MAV_SYS_STATUS_TERRAIN;
        //break;
    case AP_Terrain::TerrainStatusOK:
        control_sensors_present |= MAV_SYS_STATUS_TERRAIN;
        control_sensors_enabled |= MAV_SYS_STATUS_TERRAIN;
        control_sensors_health  |= MAV_SYS_STATUS_TERRAIN;
        break;
    }
#endif
#endif

    gcs_singleton->control_sensors_present |= MAV_SYS_STATUS_SENSOR_PROPULSION;
    gcs_singleton->control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_PROPULSION;
    // only mark propulsion healthy if all of the motors are producing
    // nominal thrust
    if (fms.motors!= NULL && !Motors_get_thrust_boost(fms.motors)) {
        gcs_singleton->control_sensors_health |= MAV_SYS_STATUS_SENSOR_PROPULSION;
    }
}

static uint16_t gcs_copter_min_loop_time_remaining_for_message_send_us(gcs_t gcs_singleton)
{
    return 250;
}

static gcs_mavlink_t gcs_copter_new_gcs_mavlink_backend(gcs_t gcs_singleton, gcs_mavlink_parameters_t params, rt_device_t uart)
{
    return gcsmav_copter_probe(params, uart);
}

static gcs_mavlink_t gcsmav_copter_probe(gcs_mavlink_parameters_t params, rt_device_t uart)
{
    gcs_mavlink_t gcs_copter = (gcs_mavlink_t)rt_malloc(sizeof(struct gcs_mavlink));

    if (!gcs_copter) {
        return NULL;
    }

    gcsmav_ctor(gcs_copter, &gcsmav_copter_ops, params, uart);

    return gcs_copter;
}

/**
  * @brief       
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  */
static void gcs_copter_assign_param()
{
    gcs_t gcs_singleton = gcs_get_singleton();

    GCS_PARAM_LINK_VARIABLE(gcs_singleton, 0);

#if MAVLINK_COMM_NUM_BUFFERS >= 2
    GCS_PARAM_LINK_VARIABLE(gcs_singleton, 1);
#endif

#if MAVLINK_COMM_NUM_BUFFERS >= 3
    GCS_PARAM_LINK_VARIABLE(gcs_singleton, 2);
#endif

#if MAVLINK_COMM_NUM_BUFFERS >= 4
    GCS_PARAM_LINK_VARIABLE(gcs_singleton, 3);
#endif

#if MAVLINK_COMM_NUM_BUFFERS >= 5
    GCS_PARAM_LINK_VARIABLE(gcs_singleton, 4);
#endif

#if MAVLINK_COMM_NUM_BUFFERS >= 6
    GCS_PARAM_LINK_VARIABLE(gcs_singleton, 5);
#endif

#if MAVLINK_COMM_NUM_BUFFERS >= 7
    GCS_PARAM_LINK_VARIABLE(gcs_singleton, 6);
#endif

    param_link_variable(PARAM_ID(GCS, SYSID_THISMAV), &sysid_this_mav);
    param_link_variable(PARAM_ID(GCS, SYSID_MYGCS), &sysid_my_gcs);
    param_link_variable(PARAM_ID(GCS, SYSID_ENFORCE), &sysid_enforce);
    param_link_variable(PARAM_ID(GCS, TELEM_DELAY), &telem_delay);
}


/// GCS Mavlink Copter

static uint8_t gcsmav_copter_sysid_my_gcs(gcs_mavlink_t gcsmav)
{
    return sysid_my_gcs;
}

static bool gcsmav_copter_sysid_enforce(gcs_mavlink_t gcsmav)
{
    return (bool)sysid_enforce;
}

static uint64_t gcsmav_copter_capabilities(gcs_mavlink_t gcsmav)
{
    return (MAV_PROTOCOL_CAPABILITY_MISSION_FLOAT |
            MAV_PROTOCOL_CAPABILITY_MISSION_INT |
            MAV_PROTOCOL_CAPABILITY_COMMAND_INT |
            MAV_PROTOCOL_CAPABILITY_SET_POSITION_TARGET_LOCAL_NED |
            MAV_PROTOCOL_CAPABILITY_SET_POSITION_TARGET_GLOBAL_INT |
            MAV_PROTOCOL_CAPABILITY_FLIGHT_TERMINATION |
            MAV_PROTOCOL_CAPABILITY_SET_ATTITUDE_TARGET |
            gcsmavCapabilities(gcsmav));
}

static uint32_t gcsmav_copter_telem_delay(gcs_mavlink_t gcsmav)
{
    return (uint32_t)(telem_delay);
}

static MAV_LANDED_STATE gcsmav_copter_landed_state(gcs_mavlink_t gcsmav)
{
    if (fms.ap.land_complete) {
        return MAV_LANDED_STATE_ON_GROUND;
    }
    if (mode_is_landing(fms.flightmode)) {
        return MAV_LANDED_STATE_LANDING;
    }
    if (mode_is_taking_off(fms.flightmode)) {
        return MAV_LANDED_STATE_TAKEOFF;
    }
    return MAV_LANDED_STATE_IN_AIR;
}

// 
static int gcs_copter_ctor(void)
{
    gcs_copter_assign_param();

    gcs_ctor(&gcs_copter_ops);

    return 0;
}
INIT_COMPONENT_EXPORT(gcs_copter_ctor);
/*------------------------------------test------------------------------------*/


